#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/msg/point_field.hpp" // 包含 PointField 消息定义
#include <mosquitto.h>
#include <jsoncpp/json/json.h> // 用于序列化元数据
#include <string>
#include <vector>
#include <memory>
#include <cstring> 

class PointCloudMqttPublisher : public rclcpp::Node
{
public:
    PointCloudMqttPublisher() : Node("pointcloud_mqtt_publisher")
    {
        // 声明参数
        mqtt_host_ = this->declare_parameter<std::string>("mqtt_net.host", "localhost");
        mqtt_port_ = this->declare_parameter<int>("mqtt_net.port", 1883);
        input_ros_topic_ = this->declare_parameter<std::string>("ros.input_topic", "/filtered_cloud");
        mqtt_data_topic_ = this->declare_parameter<std::string>("mqtt.data_topic", "/robot/pointcloud/data");
        mqtt_meta_topic_ = this->declare_parameter<std::string>("mqtt.meta_topic", "/robot/pointcloud/meta");

        // 获取参数值
        mqtt_host_ = this->get_parameter("mqtt_net.host").as_string();
        mqtt_port_ = this->get_parameter("mqtt_net.port").as_int();
        input_ros_topic_ = this->get_parameter("ros.input_topic").as_string();
        mqtt_data_topic_ = this->get_parameter("mqtt.data_topic").as_string();
        mqtt_meta_topic_ = this->get_parameter("mqtt.meta_topic").as_string();

        RCLCPP_INFO(this->get_logger(), "Input ROS Topic: %s", input_ros_topic_.c_str());
        RCLCPP_INFO(this->get_logger(), "MQTT Data Topic: %s", mqtt_data_topic_.c_str());
        RCLCPP_INFO(this->get_logger(), "MQTT Meta Topic: %s", mqtt_meta_topic_.c_str());

        // 初始化 MQTT
        mosquitto_lib_init();
        mqtt_client_ = mosquitto_new(nullptr, true, this);

        if (!mqtt_client_)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to create MQTT client");
            return;
        }

        mosquitto_connect_callback_set(mqtt_client_, on_connect_static);
        mosquitto_disconnect_callback_set(mqtt_client_, on_disconnect_static);

        connect_thread_ = std::thread(&PointCloudMqttPublisher::connect_loop, this);

        int rc = mosquitto_loop_start(mqtt_client_);
        if (rc != 0)
        {
            RCLCPP_ERROR(this->get_logger(), "Could not start MQTT loop: %s", mosquitto_strerror(rc));
            mosquitto_disconnect(mqtt_client_);
            mosquitto_destroy(mqtt_client_);
            return;
        }

        // 创建 ROS 订阅者
        pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            input_ros_topic_, 10,
            std::bind(&PointCloudMqttPublisher::pointcloud_callback, this, std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "PointCloud MQTT Publisher initialized.");
    }

    ~PointCloudMqttPublisher()
    {
        stop_connect_thread_ = true;
        if (connect_thread_.joinable())
        {
            connect_thread_.join();
        }

        if (mqtt_client_)
        {
            mosquitto_loop_stop(mqtt_client_, true);
            mosquitto_disconnect(mqtt_client_);
            mosquitto_destroy(mqtt_client_);
        }
        mosquitto_lib_cleanup();
    }

private:
    void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        if (!is_connected_)
        {
            RCLCPP_WARN(this->get_logger(), "Not connected to MQTT broker, cannot publish PointCloud2 data.");
            return;
        }

        RCLCPP_INFO(this->get_logger(), "Received PointCloud2, size: %zu bytes", msg->data.size());

        // 1. 序列化meta元数据为 JSON
        Json::Value root;
        root["header"]["stamp"]["sec"] = msg->header.stamp.sec;
        root["header"]["stamp"]["nanosec"] = msg->header.stamp.nanosec;
        root["header"]["frame_id"] = msg->header.frame_id;
        root["height"] = msg->height;
        root["width"] = msg->width;
        root["is_bigendian"] = msg->is_bigendian;
        root["point_step"] = msg->point_step;
        root["row_step"] = msg->row_step;
        root["is_dense"] = msg->is_dense;

        Json::Value fields_array(Json::arrayValue);
        for (const auto& field : msg->fields) {
            Json::Value field_obj;
            field_obj["name"] = field.name;
            field_obj["offset"] = field.offset;
            field_obj["datatype"] = field.datatype;
            field_obj["count"] = field.count;
            fields_array.append(field_obj);
        }
        root["fields"] = fields_array;

        Json::StreamWriterBuilder builder;
        builder["indentation"] = ""; // 紧凑格式
        std::string meta_json = Json::writeString(builder, root);

        // 2. 发送元数据到 MQTT
        int rc_meta = mosquitto_publish(mqtt_client_, nullptr, mqtt_meta_topic_.c_str(),
                                       meta_json.length(), meta_json.c_str(), 0, false);
        if (rc_meta != 0)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to publish metadata to MQTT: %s", mosquitto_strerror(rc_meta));
            return; // 如果元数据失败，数据包也无法解析
        }
        else
        {
            RCLCPP_DEBUG(this->get_logger(), "Published metadata to MQTT, size: %zu", meta_json.length());
        }

        // 3. 发送二进制数据到 MQTT
        int rc_data = mosquitto_publish(mqtt_client_, nullptr, mqtt_data_topic_.c_str(),
                                       msg->data.size(), reinterpret_cast<const uint8_t*>(msg->data.data()), 0, false);
        if (rc_data != 0)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to publish data to MQTT: %s", mosquitto_strerror(rc_data));
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Published data to MQTT, size: %zu bytes", msg->data.size());
        }
    }


    // --- MQTT Callbacks (Static) ---
    static void on_connect_static(struct mosquitto *mosq, void *obj, int rc)
    {
        if (!obj) return;
        static_cast<PointCloudMqttPublisher*>(obj)->on_connect(rc);
    }

    void on_connect(int rc)
    {
        if (rc == 0)
        {
            is_connected_ = true;
            RCLCPP_INFO(this->get_logger(), "Successfully connected to MQTT broker");
        }
        else
        {
            is_connected_ = false;
            RCLCPP_ERROR(this->get_logger(), "Failed to connect to MQTT broker: %s", mosquitto_strerror(rc));
        }
    }

    static void on_disconnect_static(struct mosquitto *mosq, void *obj, int rc)
    {
        if (!obj) return;
        static_cast<PointCloudMqttPublisher*>(obj)->on_disconnect(rc);
    }

    void on_disconnect(int rc)
    {
        if (rc != 0)
        {
            is_connected_ = false;
            RCLCPP_WARN(this->get_logger(), "Disconnected from MQTT broker. Will attempt to reconnect...");
        }
    }

    void connect_loop()
    {
        int retry_count = 0;
        const int max_retry_delay = 5;

        while (rclcpp::ok() && !stop_connect_thread_)
        {
            if (!is_connected_)
            {
                RCLCPP_INFO(this->get_logger(), "Trying to connect to MQTT broker: %s:%d",
                            mqtt_host_.c_str(), mqtt_port_);

                int rc = mosquitto_connect(mqtt_client_, mqtt_host_.c_str(), mqtt_port_, 60);
                if (rc != 0)
                {
                    int delay = std::min(1 << retry_count, max_retry_delay);
                    RCLCPP_ERROR(this->get_logger(), "Could not connect to MQTT broker: %s. Retrying in %d seconds...",
                                 mosquitto_strerror(rc), delay);
                    std::this_thread::sleep_for(std::chrono::seconds(delay));
                    retry_count++;
                }
                else
                {
                    std::this_thread::sleep_for(std::chrono::seconds(1));
                }
            }
            else
            {
                std::this_thread::sleep_for(std::chrono::seconds(1));
            }
        }
    }

    struct mosquitto *mqtt_client_;
    std::string input_ros_topic_;
    std::string mqtt_data_topic_;
    std::string mqtt_meta_topic_;

    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;

    bool is_connected_ = false; // Initialize
    std::thread connect_thread_;
    bool stop_connect_thread_ = false;
    std::string mqtt_host_;
    int mqtt_port_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PointCloudMqttPublisher>());
    rclcpp::shutdown();
    return 0;
}